{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Feedback Cancellation of the Double Pendulum\n",
    "\n",
    "Let's say that we would like our simple double pendulum to act like a\n",
    "simple single pendulum (with damping), whose dynamics are given by:\n",
    "\\begin{align*} \\ddot \\theta_1 &= -\\frac{g}{l}\\sin\\theta_1 -b\\dot\\theta_1 \\\\\n",
    "\\ddot\\theta_2 &= 0. \\end{align*} This is easily achieved using\n",
    "(Note that our chosen dynamics do not actually stabilize $\\theta_2$ -- this detail was left out for clarity, but would be necessary for any real\n",
    "implementation.) $${\\bf u}  = {\\bf B}^{-1}\\left[ {\\bf C}\\dot{{\\bf q}} - {\\bf \\tau}_g +\n",
    "{\\bf M}\\begin{bmatrix} -\\frac{g}{l}s_1 - b\\dot{q}_1 \\\\ 0 \\end{bmatrix}\n",
    "\\right].$$ \n",
    "\n",
    "Since we are embedding a nonlinear dynamics (not a linear one), we refer\n",
    "to this as \"feedback cancellation\", or \"dynamic inversion\".  This idea can,\n",
    "and does, make control look easy - for the special case of a fully-actuated\n",
    "deterministic system with known dynamics.  For example, it would have been\n",
    "just as easy for me to invert gravity. Observe that the control derivations\n",
    "here would not have been any more difficult if the robot had 100 joints."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the underactuated repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  Colab will ask you to \"Reset all runtimes\"; say no to save yourself the reinstall.\n",
    "- On Binder, the machines should already be provisioned by the time you can run this; it should return (almost) instantly.\n",
    "\n",
    "More details are available [here](http://underactuated.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  import pydrake\n",
    "  import underactuated\n",
    "except ImportError:\n",
    "  !curl -s https://raw.githubusercontent.com/RussTedrake/underactuated/master/scripts/setup/jupyter_setup.py > jupyter_setup.py\n",
    "  from jupyter_setup import setup_underactuated\n",
    "  setup_underactuated()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Acting like a single pendulum\n",
    "\n",
    "First we implement our simple controller as a system that takes the pendulum state in, and outputs the motor torque."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from math import sin\n",
    "from IPython import get_ipython\n",
    "\n",
    "from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,\n",
    "                         PlanarSceneGraphVisualizer, Simulator, VectorSystem)\n",
    "from underactuated import FindResource, ManipulatorDynamics\n",
    "from underactuated.jupyter import AdvanceToAndVisualize, SetupMatplotlibBackend\n",
    "plt_is_interactive = SetupMatplotlibBackend()\n",
    "\n",
    "\n",
    "class Controller(VectorSystem):\n",
    "    \"\"\"Defines a feedback controller for the double pendulum.\n",
    "\n",
    "    The controller applies torques at the joints in order to:\n",
    "\n",
    "    1) cancel out the dynamics of the double pendulum,\n",
    "    2) make the first joint swing with the dynamics of a single pendulum, and\n",
    "    3) drive the second joint towards zero.\n",
    "\n",
    "    The magnitude of gravity for the imposed single pendulum dynamics is taken\n",
    "    as a constructor argument.  So you can do fun things like pretending that\n",
    "    gravity is zero, or even inverting gravity!\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, multibody_plant, gravity):\n",
    "        # 4 inputs (double pend state), 2 torque outputs.\n",
    "        VectorSystem.__init__(self, 4, 2)\n",
    "        self.plant = multibody_plant\n",
    "        self.g = gravity\n",
    "\n",
    "    def DoCalcVectorOutput(self, context, double_pend_state, unused, torque):\n",
    "        # Extract manipulator dynamics.\n",
    "        q = double_pend_state[:2]\n",
    "        v = double_pend_state[-2:]\n",
    "        (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(self.plant, q, v)\n",
    "\n",
    "        # Desired pendulum parameters.\n",
    "        length = 2.\n",
    "        b = .1\n",
    "\n",
    "        # Control gains for stabilizing the second joint.\n",
    "        kp = 1\n",
    "        kd = .1\n",
    "\n",
    "        # Cancel double pend dynamics and inject single pend dynamics.\n",
    "        torque[:] = Cv - tauG - tauExt + M.dot(\n",
    "            [self.g / length * sin(q[0]) - b * v[0], -kp * q[1] - kd * v[1]])\n",
    "\n",
    "\n",
    "def simulate(gravity=-9.8):      \n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "\n",
    "    # Load the double pendulum from Universal Robot Description Format\n",
    "    parser = Parser(plant, scene_graph)\n",
    "    parser.AddModelFromFile(FindResource(\"models/double_pendulum.urdf\"))\n",
    "    plant.Finalize()\n",
    "\n",
    "    controller = builder.AddSystem(Controller(plant, gravity))\n",
    "    builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))\n",
    "    builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port())\n",
    "\n",
    "    visualizer = builder.AddSystem(\n",
    "        PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8], \n",
    "                                   show=plt_is_interactive))\n",
    "    builder.Connect(scene_graph.get_pose_bundle_output_port(),\n",
    "                    visualizer.get_input_port(0))\n",
    "\n",
    "    diagram = builder.Build()\n",
    "\n",
    "    # Set up a simulator to run this diagram\n",
    "    simulator = Simulator(diagram)\n",
    "\n",
    "    # Set the initial conditions\n",
    "    context = simulator.get_mutable_context()\n",
    "    context.SetContinuousState((1., 0., 0.2, 0.))  # (θ₁, θ₂, θ̇₁, θ̇₂)\n",
    "\n",
    "    # Simulate\n",
    "    duration = 3.0 if get_ipython() else 0.1 # sets a shorter duration during testing\n",
    "    AdvanceToAndVisualize(simulator, visualizer, duration)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If we simulate this system with the default parameters (gravity = -9.8m/s), then our double pendulum acts like a single pendulum."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "simulate()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But if we've gone this far, we could have replaced the dynamics with almost anything.  For instance, with a simple change, we can use feedback cancellation to invert gravity!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "simulate(gravity=9.8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}